/* 
 * File: main_lab2_wander.c
 * Author: Testuya Idota and Brian Fehrman
 * Description: Sends commands to the iRobot
 * 				from the Seeduino via USART
 * 				communication. The robot will wander
 * 				around. It will turn -90 degrees
 * 				on a front bump, and will turn
 * 				1 degree away from the direction
 * 				of a left or right bump until it no
 * 				longer contacts the obstacle.
 * Created on (date): 2-5-2013
 */
#include <avr/io.h>
#include <util/delay.h>
#include <inttypes.h>
#include "usart.h"
#include "iRobotSensors.h"
#include "iRobotControl.h"

const int DRIVE_SPEED = 500;	//Wheel speeds in mm/s

int main ( void )
{    
	//Initialize USART1
	usart_config ucon_1;
	ucon_1.number = 1;
	ucon_1.baudrate = 57600;
	configure_usart( &ucon_1 );
	
	_delay_ms( 1000 ); //wait for a second

	usart1tx( ( uint8_t ) 128 ); //Start
	
	_delay_ms( 1000 ); //wait for a second
	
	usart1tx( ( uint8_t ) 132 ); //to Full Mode
	
	_delay_ms( 1000 );
	
	//Wait until bumper states are cleared...will be an issue
	//if robot starts out with bumpers depressed
	while( 	( get_bumper_and_wheel_states( ) & 0x03) != 0 );

	while(1)
	{
		uint8_t bump_status; //used to check when bumpers depressed and which ones
		uint8_t unbump_status; //used to wait for bumpers to clear

		//Drive forward until bumper states change
		DriveRL( DRIVE_SPEED, DRIVE_SPEED );
		do
		{
			bump_status = get_bumper_and_wheel_states( );
		}while( ( bump_status & 0x03 ) == 0 );
		
		if( ( bump_status & 0x03 ) == 0x03 ) //front bump
		{
			//Turn -90 degrees and wait for bumper to clear
			//Will be a problem if it still detects a front
			//bump after turning -90
			TurnAngle( -90, DRIVE_SPEED );
			
			do
			{		
				unbump_status = get_bumper_and_wheel_states( );
			}while( ( unbump_status & 0x03 ) != 0 );
			
		}
		else if( ( bump_status & 0x03 ) == 0x01 ) //right bump
		{ 
			//Turn 1 degree until bumper states are cleared
			do
			{			
				TurnAngle( 1, DRIVE_SPEED );
				unbump_status = get_bumper_and_wheel_states( );
			}while( ( unbump_status & 0x03 ) != 0 );
		}
		else //left bump
		{ 
			//Turn -1 degree until bumper states are cleared
			do
			{			
				TurnAngle( -1, DRIVE_SPEED );
				unbump_status = get_bumper_and_wheel_states( );
			}while( ( unbump_status & 0x03 ) != 0 );
		}
	}

	return -1;
}


